#include "PID.h"
#include <stdio.h>

void PIDControl(PIDInfo_t *PID , float Error);

void PIDControl(PIDInfo_t *PID , float Error)
{
    if(PID == NULL)
    {
        return;
    }

    PID->Output += ((PID->Kp * (Error - PID->LastError))
                    + (PID->Ki * Error)
                    + (PID->Kd * (Error - (2 * PID->LastError) + PID->PreviousError)));

    PID->PreviousError = PID->LastError;
    PID->LastError = Error;
}
